#ifndef CAROS_UR_SERVICE_INTERFACE_H
#define CAROS_UR_SERVICE_INTERFACE_H

#include <caros_control_msgs/RobotState.h>
#include <caros_universalrobot/UrServiceServoQ.h>
#include <caros_universalrobot/UrServiceEmpty.h>
#include <caros_universalrobot/UrServiceForceModeUpdate.h>
#include <caros_universalrobot/UrServiceForceModeStart.h>
#include <caros_universalrobot/UrServiceForceModeStop.h>
#include <caros_universalrobot/UrServiceMoveLin.h>
#include <caros_universalrobot/UrServiceMovePtp.h>
#include <caros_universalrobot/UrServiceMovePtpPath.h>
#include <caros_universalrobot/UrServiceMoveVelQ.h>
#include <caros_universalrobot/UrServiceMoveVelT.h>
#include <caros_universalrobot/UrServicePayload.h>
#include <caros_universalrobot/UrServiceSetIO.h>

#include <rw/math/Q.hpp>
#include <rw/math/Transform3D.hpp>
#include <rw/math/VelocityScrew6D.hpp>
#include <rw/math/Wrench6D.hpp>
#include <rw/trajectory/Path.hpp>

#include <ros/ros.h>
#include <ros/callback_queue.h>

#include <string>
#include <tuple>

#define UR_STATE_PUBLISHER_QUEUE_SIZE 30 /* Do not decrease this number unless you know what you doing */
#define UR_SERVICE_INTERFACE_SUB_NAMESPACE "ur_service_interface"
#define ASYNC_SPIN_USE_THREAD_FOR_EACH_CORE 0
#define ASYNC_SPIN_SINGLE_THREAD 1 /* If necessary, use this option for reducing CPU load*/

class URServiceInterface
{
 public:
  /**
   * @brief constructor
   * @param[in] nodehandle the nodehandle to use for services.
   */
  explicit URServiceInterface(const ros::NodeHandle& nodehandle);

  /**
   * @brief virtual destructor
   */
  virtual ~URServiceInterface();

  /**
   * @brief Initialise this interface, such that the ROS services and publishers become available.
   * @returns a boolean indicating whether all the ROS services and publishers were successfully made available.
   */
  bool configureInterface();

  //! publish robot state
  void publishState(const caros_control_msgs::RobotState& state);

  //! @brief move robot on a linear Cartesian path (in meters)
  virtual bool urMoveLin(const rw::math::Transform3D<>& target, double speed, double acceleration) = 0;

  //! @brief move robot from joint space point to joint space point (in radians)
  virtual bool urMovePtp(const rw::math::Q& target, double speed, double acceleration) = 0;

  /**
   * @brief move to each joint position specified in a path. with joint positions that includes acceleration,
   * speed and blend for each position.
   */
  virtual bool urMovePtpPath(const rw::trajectory::QPath& q_path) = 0;

  //! @brief move robot in a servoing fashion specifying joint velocity targets (in radians/sec)
  virtual bool urMoveVelQ(const rw::math::Q& q_vel, double acceleration, double time) = 0;

  //! @brief move robot in a servoing fashion specifying a velocity screw in tool coordinates (in meters/sec and
  //! radians/sec)
  virtual bool urMoveVelT(const rw::math::VelocityScrew6D<>& t_vel, double acceleration, double time) = 0;

  /**
   * @brief move robot in a servoing fashion using joint configurations
   * @param[in] target The joint configurations to servo to.
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urServoQ(const rw::math::Q& target, const float time, const float lookahead_time, const float gain) = 0;

  /**
   * @brief Stop the servos
   */
  virtual bool urServoStop() = 0;

  /**
   * @brief Set robot to be controlled in force mode.
   *
   * @general 6D vector = [x, y, z, rx, ry, rz], C = compliant, NC = Non-Compliant
   * @param[in] ref_t_offset Pose vector that defines the force frame relative to the base frame.
   * @param[in] selection 6D vector, setting C in the axis direction (=1) or NC (=0)
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   * @param[in] type An integer [1;3] specifying how the robot interprets the force frame. 1: The force frame is
   * transformed in a way such that its y-axis is aligned with a vector pointing from the robot tcp towards the origin
   *of
   * the force frame. 2: The force frame is not transformed. 3: The force frame is transformed in a way such that its
   * x-axis is the projection of the robot tcp velocity vector onto the x-y plane of the force frame.
   * @param[in] limits 6D vector, C: max tcp speed, NV: max deviation from tcp pose
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeStart(const rw::math::Transform3D<>& ref_t_offset, const rw::math::Q& selection,
                                const rw::math::Wrench6D<>& wrench_target, int type, const rw::math::Q& limits) = 0;

  /**
   * @brief Update the wrench the robot will apply to its environment.
   * @param[in] wrench_target The forces/torques the robot will apply to its environment. The robot adjusts its position
   * along/about compliant axis in order to achieve the specified force/torque. Values have no effect for non-compliant
   * axes
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeUpdate(const rw::math::Wrench6D<>& wrench_target) = 0;

  /**
   * @brief Resets the robot mode from force mode to normal operation.
   *
   * @returns a boolean indicating if the serial device accepted the command
   */
  virtual bool urForceModeStop() = 0;

  /**
   * @brief Sets the payload of the robot.
   *
   * @param[in] mass The mass of the payload in kg.
   * @param[in] com The displacement of the Center of Mass from the robot TCP.
   */
  virtual bool urSetPayload(const double& mass, const rw::math::Vector3D<>& com) = 0;

  /**
   * @brief Sets the specified IO pin of the robot to high or low based on value provided.
   *
   * @param[in] pin The id of the pin to set
   * @param[in] value The value determines if the pin should be high or low (true means the output is high)
   */
  virtual bool urSetIO(const int& pin, const bool& value) = 0;

  /**
   * @brief Stop the robot and script running on the controller
   */
  virtual bool urMoveStop() = 0;

 private:
  bool urServoStopHandle(caros_universalrobot::UrServiceEmpty::Request& request,
                         caros_universalrobot::UrServiceEmpty::Response& response);

  bool urServoQHandle(caros_universalrobot::UrServiceServoQ::Request& request,
                      caros_universalrobot::UrServiceServoQ::Response& response);

  bool urForceModeStartHandle(caros_universalrobot::UrServiceForceModeStart::Request& request,
                              caros_universalrobot::UrServiceForceModeStart::Response& response);

  bool urForceModeUpdateHandle(caros_universalrobot::UrServiceForceModeUpdate::Request& request,
                               caros_universalrobot::UrServiceForceModeUpdate::Response& response);

  bool urForceModeStopHandle(caros_universalrobot::UrServiceForceModeStop::Request& request,
                             caros_universalrobot::UrServiceForceModeStop::Response& response);

  bool urMoveLinHandle(caros_universalrobot::UrServiceMoveLin::Request& request,
                       caros_universalrobot::UrServiceMoveLin::Response& response);

  bool urMovePtpHandle(caros_universalrobot::UrServiceMovePtp::Request& request,
                       caros_universalrobot::UrServiceMovePtp::Response& response);

  bool urMovePtpPathHandle(caros_universalrobot::UrServiceMovePtpPath::Request& request,
                           caros_universalrobot::UrServiceMovePtpPath::Response& response);

  bool urMoveVelQHandle(caros_universalrobot::UrServiceMoveVelQ::Request& request,
                        caros_universalrobot::UrServiceMoveVelQ::Response& response);

  bool urMoveVelTHandle(caros_universalrobot::UrServiceMoveVelT::Request& request,
                        caros_universalrobot::UrServiceMoveVelT::Response& response);

  bool urMoveStopHandle(caros_universalrobot::UrServiceEmpty::Request& request,
                        caros_universalrobot::UrServiceEmpty::Response& response);

  bool urSetPayloadHandle(caros_universalrobot::UrServicePayload::Request& request,
                          caros_universalrobot::UrServicePayload::Response& response);

  bool urSetIOHandle(caros_universalrobot::UrServiceSetIO::Request& request,
                     caros_universalrobot::UrServiceSetIO::Response& response);

 protected:
  ros::NodeHandle nodehandle_;
  ros::CallbackQueue ur_service_cb_queue_;
  ros::AsyncSpinner ur_service_async_spinner_;
  ros::Publisher ur_state_publisher_;

  ros::AdvertiseServiceOptions srv_ur_servo_stop_ops_;
  ros::AdvertiseServiceOptions srv_ur_servo_q_ops_;
  ros::AdvertiseServiceOptions srv_ur_force_mode_start_ops_;
  ros::AdvertiseServiceOptions srv_ur_force_mode_update_ops_;
  ros::AdvertiseServiceOptions srv_ur_force_mode_stop_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_lin_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_ptp_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_ptp_path_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_vel_q_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_vel_t_ops_;
  ros::AdvertiseServiceOptions srv_ur_move_stop_ops_;
  ros::AdvertiseServiceOptions srv_ur_set_payload_ops_;
  ros::AdvertiseServiceOptions srv_ur_set_io_ops_;

  ros::ServiceServer srv_ur_servo_stop_;
  ros::ServiceServer srv_ur_servo_q_;
  ros::ServiceServer srv_ur_force_mode_start_;
  ros::ServiceServer srv_ur_force_mode_update_;
  ros::ServiceServer srv_ur_force_mode_stop_;
  ros::ServiceServer srv_ur_move_lin_;
  ros::ServiceServer srv_ur_move_ptp_;
  ros::ServiceServer srv_ur_move_ptp_path_;
  ros::ServiceServer srv_ur_move_vel_q_;
  ros::ServiceServer srv_ur_move_vel_t_;
  ros::ServiceServer srv_ur_move_stop_;
  ros::ServiceServer srv_ur_set_payload_;
  ros::ServiceServer srv_ur_set_io_;
};

#endif  // CAROS_UR_SERVICE_INTERFACE_H
